/**
 *******************************************************************************
 * @FileName  : sdp.c
 * @Author    : GaoQiu
 * @CreateDate: 2020-05-18
 * @Copyright : Copyright(C) GaoQiu
 *              All Rights Reserved.
 *******************************************************************************
 *
 * The information contained herein is confidential and proprietary property of
 * GaoQiu and is available under the terms of Commercial License Agreement
 * between GaoQiu and the licensee in separate contract or the terms described
 * here-in.
 *
 * This heading MUST NOT be removed from this file.
 *
 * Licensees are granted free, non-transferable use of the information in this
 * file under Mutual Non-Disclosure Agreement. NO WARRENTY of ANY KIND is provided.
 *
 *******************************************************************************
 */
#include "sdp.h"
#include "utils/byte_stream.h"
#include "utils/lg_mem.h"

#include "stack/host/att_api.h"
#include "stack/host/att_uuid.h"

#define SDP_TIMEOUT_SEC     15

SDP_EventCback_t  SDP_EvtCback = NULL;

static void SDP_ServiceSearchEndHandler(SdpCb_t *pSdpCb);
static void SDP_CharSearchEndHandler(SdpCb_t *pSdpCb);
static void SDP_CCCDSearchEndHandler(SdpCb_t *pSdpCb);

static void SDP_StartCharactDeclareSearch(SdpCb_t *pSdpCb);
static void SDP_StartCCCDSearch(SdpCb_t *pSdpCb);

void SDP_RegisterEvtCback(SDP_EventCback_t cback)
{
	SDP_EvtCback = cback;
}

void SDP_ExecEvtCback(SdpCb_t *pSdpCb, uint8_t evtId)
{
	SdpEvt_t sdpEvt;
	sdpEvt.connHandle = pSdpCb->connHandle;
	sdpEvt.evtId = evtId;


	switch(evtId)
	{
	case SDP_EVT_SERVER_SEARCH_START:
		SDP_TRACK_WRN("SDP Services Search Start\r\n");
		break;

	case SDP_EVT_SERVER_SEARCH_END:
		SDP_TRACK_WRN("SDP Services Search End...\r\n");
		break;

	case SDP_EVT_CHAR_DECLARE_SEARCH_START:
		SDP_TRACK_WRN("SDP Charact Declare Search Start\r\n");
		break;

	case SDP_EVT_CHAR_DECLARE_SEARCH_END:
		SDP_TRACK_WRN("SDP Charact Declare Search End...\r\n");
		break;

	case SDP_EVT_CCCD_SEARCH_START:
		SDP_TRACK_WRN("SDP CCCD Search Start\r\n");
		break;

	case SDP_EVT_CCCD_SEARCH_END:
		SDP_TRACK_WRN("SDP CCCD Search End...\r\n");
		break;

	case SDP_EVT_TIMEOUT:
		SDP_TRACK_ERR("SDP Timeout...\r\n");
		break;

	case SDP_EVT_SDP_END:
		SDP_TRACK_INFO("******SDP End******\r\n");
		break;

	default:
		break;
	}

	if(SDP_EvtCback){
		SDP_EvtCback(&sdpEvt);
	}
}

/**
 * @brief : SDP Init
 * @parem :
 * @return:
 */
void SDP_Init(SdpCb_t *pSdpCb, uint16_t connHandle)
{
	//memset(&pSdpCb->attGroup, 0, sizeof(pSdpCb->attGroup));
	pSdpCb->attGrpCnt = 0;

	pSdpCb->connHandle = connHandle;//[!!! important]

	pSdpCb->curGrpCnt = 0;
	pSdpCb->curAttCnt = 0;

	pSdpCb->sdpState  = SDP_STA_NONE;

	pSdpCb->timeoutTimer = 0;

	for(int j=0; j<APP_MAX_SERVICE_NUM; j++)
	{
		AttGroup_t *pGrp = &pSdpCb->attGroup[j];
		pGrp->attsCnt = 0;
		pGrp->maxAttNum = APP_MAX_CHAR_DECLARE_NUM;
	#if APP_NEW_SDP_EN
		pGrp->maxAttNum = 0;
	#endif
	}

	pSdpCb->enable = false;
}

void SDP_ParamReset(SdpCb_t *pSdpCb)
{
	pSdpCb->connHandle = 0xFFFF; //invalid handle;

	pSdpCb->attGrpCnt = 0;

	pSdpCb->curGrpCnt = 0;
	pSdpCb->curAttCnt = 0;

	pSdpCb->sdpState  = SDP_STA_NONE;

	pSdpCb->timeoutTimer = 0;

	for(int j=0; j<APP_MAX_SERVICE_NUM; j++)
	{
		AttGroup_t *pGrp = &pSdpCb->attGroup[j];
		pGrp->attsCnt = 0;

	#if APP_NEW_SDP_EN
		if(pGrp->pAtts){
			//Free memory
			lg_mem_release(pGrp->pAtts);
			pGrp->pAtts = NULL;
		}
		pGrp->maxAttNum = 0;
	#else
		pGrp->maxAttNum = APP_MAX_CHAR_DECLARE_NUM;
	#endif
	}
	pSdpCb->enable = false;
}

bool_t SDP_IsEnable(SdpCb_t *pSdpCb)
{
	return pSdpCb->enable;
}

uint8_t SDP_GetState(SdpCb_t *pSdpCb)
{
	return pSdpCb->sdpState;
}

bool_t SDP_StateIs(SdpCb_t *pSdpCb, uint8_t state)
{
	return (pSdpCb->sdpState == state);
}

void SDP_End(SdpCb_t *pSdpCb)
{
	pSdpCb->timeoutTimer = 0;
	pSdpCb->enable = false;
	pSdpCb->sdpState = SDP_STA_END;
	SDP_ExecEvtCback(pSdpCb, SDP_EVT_SDP_END);
}

void SDP_TimeoutHandler(SdpCb_t *pSdpCb)
{
	pSdpCb->timeoutTimer = 0;
	pSdpCb->enable = false;
	pSdpCb->sdpState = SDP_STA_NONE;
	SDP_ExecEvtCback(pSdpCb, SDP_EVT_TIMEOUT);
}

BleState_t SDP_SearchPrimServices(uint16_t connHandle, uint16_t startHandle, uint16_t endHandle)
{
	uint8_t grpUuid[] = {UINT16_TO_BYTES(ATT_UUID_PRIMARY_SERVICE)};
	return ATTC_SendReadByGroupTypeReq(connHandle, startHandle, endHandle, grpUuid, sizeof(grpUuid));
}

BleState_t SDP_SearchCharDeclare(uint16_t connHandle, uint16_t startHandle, uint16_t endHandle)
{
	uint8_t charUuid[] = {UINT16_TO_BYTES(ATT_UUID_CHARACTERISTIC)};
	return ATTC_SendReadByTypeReq(connHandle, startHandle, endHandle, charUuid, sizeof(charUuid));
}

BleState_t SDP_SearchCCCD(uint16_t connHandle, uint16_t startHandle, uint16_t endHandle)
{
	uint8_t cccdUuid[] = {UINT16_TO_BYTES(ATT_UUID_CLIENT_CHAR_CONFIG)};
	return ATTC_SendReadByTypeReq(connHandle, startHandle, endHandle, cccdUuid, sizeof(cccdUuid));
}

BleState_t SDP_SetCCCD(uint16_t connHandle, uint16_t attHandle, uint16_t value)
{
	return ATTC_SendWriteCmd(connHandle, attHandle, (uint8_t *)&value, 2);
}

bool_t SDP_StartNextCCCDSearch(SdpCb_t *pSdpCb)
{
	uint16_t connHandle = pSdpCb->connHandle;
	AttGroup_t *pGrp = NULL;

	while(pSdpCb->curGrpCnt < pSdpCb->attGrpCnt)
	{
		pGrp = &pSdpCb->attGroup[pSdpCb->curGrpCnt];

		uint16_t id = pSdpCb->curAttCnt;
		Att_t *pAttNext = NULL;

		while(id < pGrp->attsCnt)
		{
		#if APP_NEW_SDP_EN
			Att_t *pAtt = &pGrp->pAtts[id];
		#else
			Att_t *pAtt = &pGrp->atts[id];
		#endif
			if(pAtt->prop & (ATT_PROP_NOTIFY | ATT_PROP_INDICATE))
			{
				BleState_t res = BLE_OK;
				uint16_t attHandleEnd = 0;
				if(id == pGrp->attsCnt-1){
					res = SDP_SearchCCCD(connHandle, pAtt->attHandle, pGrp->endHandle);
					attHandleEnd = pGrp->endHandle;
				}
				else{
					pAttNext = pAtt+1;
					res = SDP_SearchCCCD(connHandle, pAtt->attHandle, pAttNext->attHandle);
					attHandleEnd = pAttNext->attHandle;
				}

				if(res == BLE_OK){
					pSdpCb->sdpState = SDP_STA_SEARCH_CCCD_WAIT;
				}
				else{
					pSdpCb->sdpState = SDP_STA_SEARCH_CCCD_START;
					pSdpCb->attHandleStart = pAtt->attHandle;
					pSdpCb->attHandleEnd = attHandleEnd;
				}

				pSdpCb->curAttCnt = id;

				//SDP_TRACK_INFO("curGrpCnt:%d, curAttCnt:%d, cccdHandle:0x%02x\r\n",
				//		pSdpCb->curGrpCnt, pSdpCb->curAttCnt, 0x00);

				return true;//[!!! important]
			}
			id++;
		}
		pSdpCb->curGrpCnt++;
		pSdpCb->curAttCnt = 0;
	}

	return false;
}

void SDP_Start(SdpCb_t *pSdpCb)
{
	if(pSdpCb == NULL){
		return;
	}
	SDP_TRACK_INFO("******SDP Start******\r\n");

	uint16_t connHandle = pSdpCb->connHandle;

	if(BLE_OK == SDP_SearchPrimServices(connHandle, 0x0001, 0xFFFF)){
		pSdpCb->sdpState = SDP_STA_SEARCH_SERVICE_WAIT;
		SDP_ExecEvtCback(pSdpCb, SDP_EVT_SERVER_SEARCH_START);
	}
	else{
		pSdpCb->sdpState = SDP_STA_SEARCH_SERVICE_START;
		pSdpCb->attHandleStart = 0x0001;
		pSdpCb->attHandleEnd   = 0xFFFF;
	}

	/* Start Timeout Timer */
	pSdpCb->timeoutTimer = GetCurSysTick()|1;

	/* Enable SDP */
	pSdpCb->enable = true;
}

void SDP_StartCharactDeclareSearch(SdpCb_t *pSdpCb)
{
	if(pSdpCb == NULL){
		return;
	}
	uint16_t connHandle = pSdpCb->connHandle;

	if(pSdpCb->attGrpCnt == 0){
		SDP_End(pSdpCb);
		return;
	}

	/* Reset Parameter */
	pSdpCb->curGrpCnt = 0;

	AttGroup_t *pGrp = &pSdpCb->attGroup[pSdpCb->curGrpCnt];

	if(BLE_OK != SDP_SearchCharDeclare(connHandle, pGrp->startHandle, pGrp->endHandle)){
		pSdpCb->sdpState = SDP_STA_SEARCH_CHAR_START;
		pSdpCb->attHandleStart = pGrp->startHandle;
		pSdpCb->attHandleStart = pGrp->endHandle;
	}
	else{
		pSdpCb->sdpState = SDP_STA_SEARCH_CHAR_WAIT;
		SDP_ExecEvtCback(pSdpCb, SDP_EVT_CHAR_DECLARE_SEARCH_START);
	}

	SDP_TRACK_INFO("* ServiceGrpCnt:%d startHandle:%d, endHandle:%d\r\n",
			        pSdpCb->curGrpCnt+1, pGrp->startHandle, pGrp->endHandle);
}

void SDP_StartCCCDSearch(SdpCb_t *pSdpCb)
{
	if(pSdpCb == NULL)
		return;

	if(pSdpCb->attGrpCnt == 0){
		SDP_End(pSdpCb);
		return;
	}

	/* reset parameter */
	pSdpCb->curGrpCnt = 0;
	pSdpCb->curAttCnt = 0;

	if(SDP_StartNextCCCDSearch(pSdpCb) == true){
		SDP_ExecEvtCback(pSdpCb, SDP_EVT_CCCD_SEARCH_START);
	}
	else{
		SDP_End(pSdpCb);
	}
}

static void SDP_ServiceSearchHandler(SdpCb_t *pSdpCb, uint8_t *pAttSdu, uint16_t len)
{
	uint8_t *pPkt = pAttSdu;

	//if(len == 0 || pSdpCb == NULL || pSdpCb->sdpState != SDP_STA_SEARCH_SERVICE_WAIT){
	//	return;
	//}

	SDP_TRACK_INFO("Received ATT_READ_BY_GROUP_TYPE_RSP:\r\n");
	SDP_TRACK_DATA(pPkt, len);

	pPkt++; //skip opcode
	len--;

	uint8_t attDatLen = 0;
	BSTREAM_TO_UINT8(attDatLen, pPkt);
	len--;

	uint16_t connHandle = pSdpCb->connHandle;
	uint16_t nextHandle = 0;
	while(len > 0)
	{
		if(pSdpCb->attGrpCnt >= APP_MAX_SERVICE_NUM){
			//Start character search
			SDP_ExecEvtCback(pSdpCb, SDP_EVT_SERVER_SEARCH_END);
			SDP_StartCharactDeclareSearch(pSdpCb);
			return;
		}

		AttGroup_t *pGrp = &pSdpCb->attGroup[pSdpCb->attGrpCnt];
		BSTREAM_TO_UINT16(pGrp->startHandle, pPkt);
		BSTREAM_TO_UINT16(pGrp->endHandle, pPkt);

		uint8_t uuidLen = attDatLen - 4;
		if(uuidLen == ATT_16_UUID_LEN || uuidLen == ATT_128_UUID_LEN)
		{
			memcpy(pGrp->svcUuid.uuid_128, pPkt, attDatLen - 4);
			pGrp->svcUuid.len = uuidLen;
			pSdpCb->attGrpCnt++;
			nextHandle = pGrp->endHandle;

		#if APP_NEW_SDP_EN
			uint16_t attNum = pGrp->endHandle - pGrp->startHandle;
			pGrp->maxAttNum = (attNum / 2);
			if(attNum % 2) pGrp->maxAttNum++;
			pGrp->pAtts = (Att_t *)lg_mem_new(pGrp->maxAttNum * sizeof(Att_t));
			pGrp->attsCnt = 0;
			ASSERT(pGrp->pAtts != NULL, SDP_ERR_ALLOC_ATT_BUF_FAILED|(pGrp->maxAttNum * sizeof(Att_t)));
		#endif
		}

		pPkt += (attDatLen - 4);

		len = (len >= attDatLen) ? (len - attDatLen):0;

		SDP_TRACK_INFO("* ServiceGrpCnt:%d startHandle:%d, endHandle:%d\r\n",
				       pSdpCb->attGrpCnt,
				       pSdpCb->attGroup[pSdpCb->attGrpCnt-1].startHandle,
				       pSdpCb->attGroup[pSdpCb->attGrpCnt-1].endHandle);
	}

	if((nextHandle + 1) < 0xFFFF){
		/* Search Next Service */
		if(BLE_OK != SDP_SearchPrimServices(connHandle, (nextHandle + 1), 0xFFFF)){
			pSdpCb->sdpState = SDP_STA_SEARCH_SERVICE_START;
			pSdpCb->attHandleStart = (nextHandle + 1);
			pSdpCb->attHandleEnd = 0xFFFF;
		}
	}
	else{
		/* Start Characteristic Declare Search */
		SDP_StartCharactDeclareSearch(pSdpCb);
	}
}

static void SDP_CharDeclareSearchHandler(SdpCb_t *pSdpCb, uint8_t *pAttSdu, uint16_t len)
{
	uint8_t *pPkt = pAttSdu;

	//if(len == 0 || pSdpCb == NULL || pSdpCb->sdpState != SDP_STA_SEARCH_CHAR_WAIT){
	//	return;
	//}

	SDP_TRACK_INFO("Received ATT_READ_BY_TYPE_RSP:\r\n");
	SDP_TRACK_DATA(pPkt, len);

	pPkt++; //skip opcode
	len--;

	uint8_t attDatLen = 0;
	BSTREAM_TO_UINT8(attDatLen, pPkt);
	len--;

	uint16_t connHandle = pSdpCb->connHandle;
	AttGroup_t *pGrp = &pSdpCb->attGroup[pSdpCb->curGrpCnt];
	uint16_t nextHandle = 0;
	while(len > 0)
	{
		if(pGrp->attsCnt >= pGrp->maxAttNum){
			//TODO: start search desc
			SDP_CharSearchEndHandler(pSdpCb);
			return;
		}

	#if APP_NEW_SDP_EN
		Att_t *pAtt = &pGrp->pAtts[pGrp->attsCnt];
	#else
		Att_t *pAtt = &pGrp->atts[pGrp->attsCnt];
	#endif
		BSTREAM_TO_UINT16(pAtt->attHandle, pPkt);
		BSTREAM_TO_UINT8(pAtt->prop, pPkt);
		BSTREAM_TO_UINT16(pAtt->valueHandle, pPkt);

		uint16_t uuidLen = attDatLen - 5;
		if(uuidLen == ATT_16_UUID_LEN || uuidLen == ATT_128_UUID_LEN){
			memcpy(pAtt->valueUuid.uuid_128, pPkt, uuidLen);
			pAtt->valueUuid.len = uuidLen;
			pGrp->attsCnt++;
		}
		pPkt += uuidLen;
		nextHandle = pAtt->attHandle+1;

		len = (len >= attDatLen) ? (len - attDatLen):0;

		SDP_TRACK_INFO("** id:%d, valueHandle:%d\r\n", pGrp->attsCnt, pAtt->valueHandle);
	}

	if((nextHandle+1) >= pGrp->endHandle)
	{
		SDP_CharSearchEndHandler(pSdpCb);
	}
	else{
		if(BLE_OK != SDP_SearchCharDeclare(connHandle, nextHandle+1, pGrp->endHandle)){
			pSdpCb->sdpState = SDP_STA_SEARCH_CHAR_START;
			pSdpCb->attHandleStart = nextHandle+1;
			pSdpCb->attHandleEnd = pGrp->endHandle;
		}
	}
}

static void SDP_CCCDSearchHandler(SdpCb_t *pSdpCb, uint8_t *pAttSdu, uint16_t len)
{
	uint8_t *pPkt = pAttSdu;

	//if(len == 0 || pSdpCb == NULL || pSdpCb->sdpState != SDP_STA_SEARCH_CCCD_WAIT){
	//	return;
	//}
	SDP_TRACK_INFO("Received ATT_READ_BY_TYPE_RSP:\r\n");
	SDP_TRACK_DATA(pPkt, len);

	BleState_t ret = BLE_OK;
	uint8_t findNext = true;

	pPkt++; //skip opcode
	len--;

	uint8_t attDatLen = 0;
	BSTREAM_TO_UINT8(attDatLen, pPkt);
	len--;

	uint16_t connHandle = pSdpCb->connHandle;
	AttGroup_t *pGrp = &pSdpCb->attGroup[pSdpCb->curGrpCnt];
	while(len)
	{
		/* Save CCCD handle */
	#if APP_NEW_SDP_EN
		Att_t *pAtt = &pGrp->pAtts[pSdpCb->curAttCnt];
	#else
		Att_t *pAtt = &pGrp->atts[pSdpCb->curAttCnt];
	#endif
		BSTREAM_TO_UINT16(pAtt->cccdHandle, pPkt);

		SDP_TRACK_INFO("* ServiceGrpCnt:%d, curAttCnt:%d, cccdHandle:%d\r\n",pSdpCb->curGrpCnt, pSdpCb->curAttCnt, pAtt->cccdHandle);
		pSdpCb->curAttCnt++;//[!!! update ATT id]

		/* TODO: Enable CCCD */
		if(pAtt->prop & ATT_PROP_NOTIFY){
			if(BLE_OK != SDP_SetCCCD(connHandle, pAtt->cccdHandle, NOTIFY_ENABLE)){
				pSdpCb->sdpState = SDP_STA_SET_CCC_START;
				pSdpCb->attHandleStart = pAtt->cccdHandle;
				pSdpCb->attHandleEnd = NOTIFY_ENABLE;  /* !!! Note: Use "pSdpCb->attHandleEnd" to temporarily store parameters(Notify and Indicator). */
				findNext = false;
			}
		}
		else if(pAtt->prop & ATT_PROP_INDICATE){
			if(BLE_OK != SDP_SetCCCD(connHandle, pAtt->cccdHandle, INDICATE_ENABLE)){
				pSdpCb->sdpState = SDP_STA_SET_CCC_START;
				pSdpCb->attHandleStart = pAtt->cccdHandle;
				pSdpCb->attHandleEnd = INDICATE_ENABLE;  /* !!! Note: Use "pSdpCb->attHandleEnd" to temporarily store parameters(Notify and Indicator). */
				findNext = false;
			}
		}
		else{
			ret = 1;//error debug
		}

		SDP_TRACK_INFO("** Set CCC state: %s\r\n", ret == 0 ? "OK":"FAIL");
		break;
	}

	/* Search Next CCCD */
	if(findNext){
		SDP_CCCDSearchEndHandler(pSdpCb);
	}
}

void SDP_GattDataHandler(SdpCb_t *pSdpCb, uint8_t *pAttSdu, uint16_t len)
{
	if(pSdpCb == NULL || !SDP_IsEnable(pSdpCb) || pAttSdu == NULL || len == 0)
		return;

	if(SDP_StateIs(pSdpCb, SDP_STA_SEARCH_SERVICE_WAIT)){
		SDP_ServiceSearchHandler(pSdpCb, pAttSdu, len);
	}
	else if(SDP_StateIs(pSdpCb, SDP_STA_SEARCH_CHAR_WAIT)){
		SDP_CharDeclareSearchHandler(pSdpCb, pAttSdu, len);
	}
	else if(SDP_StateIs(pSdpCb, SDP_STA_SEARCH_CCCD_WAIT)){
		SDP_CCCDSearchHandler(pSdpCb, pAttSdu, len);
	}
}

static void SDP_ServiceSearchEndHandler(SdpCb_t *pSdpCb)
{
	//if(pSdpCb->sdpState == SDP_STA_SEARCH_SERVICE_WAIT)
	{
		if(pSdpCb->attGrpCnt == 0){
			SDP_End(pSdpCb);
		}
		else{
			SDP_ExecEvtCback(pSdpCb, SDP_EVT_SERVER_SEARCH_END);
			SDP_StartCharactDeclareSearch(pSdpCb);
		}
	}
}

static void SDP_CharSearchEndHandler(SdpCb_t *pSdpCb)
{
	uint16_t connHandle = pSdpCb->connHandle;

	//if(pSdpCb->sdpState == SDP_STA_SEARCH_CHAR_WAIT)
	{
		pSdpCb->curGrpCnt++;
		if(pSdpCb->curGrpCnt >= pSdpCb->attGrpCnt){
			SDP_ExecEvtCback(pSdpCb, SDP_EVT_CHAR_DECLARE_SEARCH_END);

			/* Start CCCD search */
			SDP_StartCCCDSearch(pSdpCb);
		}
		else{
			/* Start Next Char Declare search */
			AttGroup_t *pGrp = &pSdpCb->attGroup[pSdpCb->curGrpCnt];
			if(BLE_OK != SDP_SearchCharDeclare(connHandle, pGrp->startHandle, pGrp->endHandle)){
				pSdpCb->sdpState = SDP_STA_SEARCH_CHAR_START;
				pSdpCb->attHandleStart = pGrp->startHandle;
				pSdpCb->attHandleEnd = pGrp->endHandle;
			}

			SDP_TRACK_INFO("* ServiceGrpCnt:%d startHandle:%d, endHandle:%d\r\n",
					        pSdpCb->curGrpCnt+1, pGrp->startHandle, pGrp->endHandle);
		}
	}
}

static void SDP_CCCDSearchEndHandler(SdpCb_t *pSdpCb)
{
	//if(pSdpCb->sdpState == SDP_STA_SEARCH_CCCD_WAIT)
	{
		if(SDP_StartNextCCCDSearch(pSdpCb) == false){
			SDP_ExecEvtCback(pSdpCb, SDP_EVT_CCCD_SEARCH_END);
			SDP_End(pSdpCb);
		}
	}
}

void SDP_GattErrRspHandler(SdpCb_t *pSdpCb, uint8_t *pAttSdu, uint32_t len)
{
	if(pSdpCb == NULL ||!SDP_IsEnable(pSdpCb))
		return;

	/*!!! important */
	if(pAttSdu[1] != ATT_READ_BY_GROUP_TYPE_REQ && pAttSdu[1] != ATT_READ_BY_TYPE_REQ)
		return;

	SMP_TRACK_INFO("Received ATT_ERR_RSP:\r\n");
	SMP_TRACK_DATA(pAttSdu, len);

	if(SDP_StateIs(pSdpCb, SDP_STA_SEARCH_SERVICE_WAIT)){
		SDP_ServiceSearchEndHandler(pSdpCb);
	}
	else if(SDP_StateIs(pSdpCb, SDP_STA_SEARCH_CHAR_WAIT)){
		SDP_CharSearchEndHandler(pSdpCb);
	}
	else if(SDP_StateIs(pSdpCb, SDP_STA_SEARCH_CCCD_WAIT)){
		SDP_CCCDSearchEndHandler(pSdpCb);
	}
}

AttGroup_t *SDP_FindService(SdpCb_t *pSdpCb, uint16_t attHandle)
{
	AttGroup_t *pAttGrp = &pSdpCb->attGroup[0];
	for(int i=0; i<pSdpCb->attGrpCnt; i++, pAttGrp++)
	{
		if(pAttGrp->startHandle == attHandle){
			return pAttGrp;
		}
	}

	return NULL;
}

Att_t *SDP_FindChar(SdpCb_t *pSdpCb, uint16_t attHandle)
{
	Att_t *pAtt = NULL;
	int i=0;
	for(i=0; i<pSdpCb->attGrpCnt; i++)
	{
		AttGroup_t *pAttGrp = &pSdpCb->attGroup[i];
		if(attHandle >= pAttGrp->startHandle && attHandle <= pAttGrp->endHandle)
		{
			uint32_t cnt = 0;
			while(cnt < pAttGrp->attsCnt)
			{
			#if APP_NEW_SDP_EN
				pAtt = &pAttGrp->pAtts[cnt++];
			#else
				pAtt = &pAttGrp->atts[cnt++];
			#endif
				if(pAtt->valueHandle == attHandle){
					return pAtt;
				}
			}
		}
	}

	return NULL;
}

int SDP_DataHandler(SdpCb_t *pSdpCb, uint8_t type, uint8_t *pValue, uint32_t len)
{
	int rc = 0;
	if(pSdpCb == NULL || pValue == NULL || len == 0)
		return rc;

	switch(type)
	{
	case ATTC_ERR_RSP:
		SDP_GattErrRspHandler(pSdpCb, pValue, len);
		rc = 1;
		break;

	case ATTC_READ_BY_GROUP_TYPE_RSP:
	case ATTC_READ_BY_TYPE_RSP:
		SDP_GattDataHandler(pSdpCb, pValue, len);
		rc = 1;
		break;

	default:
		break;
	}

	return rc;
}

void SDP_Handler(SdpCb_t *pSdpCb)
{
	if(pSdpCb == NULL || !SDP_IsEnable(pSdpCb))
		return;

	switch(pSdpCb->sdpState)
	{
	case SDP_STA_SEARCH_SERVICE_START:
		if(BLE_OK == SDP_SearchPrimServices(pSdpCb->connHandle, pSdpCb->attHandleStart, pSdpCb->attHandleEnd)){
			pSdpCb->sdpState = SDP_STA_SEARCH_SERVICE_WAIT;
			SDP_ExecEvtCback(pSdpCb, SDP_EVT_SERVER_SEARCH_START);
		}
		break;

	case SDP_STA_SEARCH_CHAR_START:
		if(BLE_OK == SDP_SearchCharDeclare(pSdpCb->connHandle, pSdpCb->attHandleStart, pSdpCb->attHandleEnd)){
			pSdpCb->sdpState = SDP_STA_SEARCH_CHAR_WAIT;
			SDP_ExecEvtCback(pSdpCb, SDP_EVT_CHAR_DECLARE_SEARCH_START);
		}
		break;

	case SDP_STA_SEARCH_CCCD_START:
		if(BLE_OK == SDP_SearchCCCD(pSdpCb->connHandle, pSdpCb->attHandleStart, pSdpCb->attHandleEnd)){
			pSdpCb->sdpState = SDP_STA_SEARCH_CCCD_WAIT;
			SDP_ExecEvtCback(pSdpCb, SDP_EVT_CCCD_SEARCH_START);
		}
		break;

	case SDP_STA_SET_CCC_START:
		/* !!! Note: Use "pSdpCb->attHandleEnd" to temporarily store parameters(Notify and Indicator). */
		if(BLE_OK == SDP_SetCCCD(pSdpCb->connHandle, pSdpCb->attHandleStart, pSdpCb->attHandleEnd)){
			pSdpCb->sdpState = SDP_STA_SEARCH_CCCD_WAIT;
			/* Search Next CCCD */
			SDP_CCCDSearchEndHandler(pSdpCb);
		}
		break;

	default:
		break;
	}

	/* SDP Timeout Process. */
	if(pSdpCb->timeoutTimer && IsTimeExpired(pSdpCb->timeoutTimer, SDP_TIMEOUT_SEC*1000*1000))
	{
		SDP_TimeoutHandler(pSdpCb);
	}
}
